#include <cstdlib>
#include <stdio.h>
#include <iostream>
#include <exception>
#include <new>
#include <math.h>
#include "location.h"
#include "element.h"
#define ABS(a) ((a)<0?-(a):a)
#define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define MAX(a, b)  (((a) > (b)) ? (a) : (b))
#define DIFF_DIST 25
#define MAX_DEPTH 3
#define GOAL 0		//if 0 enemy goal at (0,750)
			//else enemy goal at (1800,750)
#define MAX_Y
#define MAX_X

int minimax(Element *CurrentN, int depth, int step);
int move_size(Element *CurrentN);
void adjustlocation(Element *node, int step, int flag);		
void adjust_angle(Element *node,int step, int flag);
float angle( int from_x, int from_y, int to_x, int to_y);
int check_parents(Element *CurrentN);
void calculate_cost(Element *CurrentN);
int del_element(Element *CurrentN);
int print_element(Element *CurrentN);
int dist(int x1, int y1, int x2, int y2);
 


int GLO_del = 0;
int GLO_nr = 0;


int main( int argc, char* argv[])
{
	Element *testN;
	testN = new Element();
	testN->own_robot.setTestLocation(500, 300, 90);
	testN->enemy_robot.setTestLocation(100, 750, 270);
	testN->last_move = 5;
	testN->depth = 0;
	testN->nr = 0;
	GLO_nr=1;
	GLO_del = 0;

	
	minimax(testN, 0, move_size(testN));
	//print_element(testN);
	
	printf("\n\ntotal #nodes : %d\n",GLO_nr);
	printf("deleted #nodes :%d\n",GLO_del);

	del_element(testN);


}

int dist(int x1, int y1, int x2, int y2)
{
	return (int) sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));

}

int print_element(Element *CurrentN)
{
	if(CurrentN == NULL)
		return 0;
		
	print_element(CurrentN->next[0]);
	print_element(CurrentN->next[1]);
	print_element(CurrentN->next[2]);
	print_element(CurrentN->next[3]);
	print_element(CurrentN->next[4]);
	printf("%d;",CurrentN->nr);
	printf("%d;%d\n",CurrentN->own_robot.getX(), CurrentN->own_robot.getY());


}


int del_element(Element *CurrentN)
{
	if(CurrentN == NULL)
		return 0;
	if(CurrentN->depth < MAX_DEPTH)
	{
		del_element(CurrentN->next[0]);
		del_element(CurrentN->next[1]);
		del_element(CurrentN->next[2]);
		del_element(CurrentN->next[3]);
		del_element(CurrentN->next[4]);
	}	
	delete(CurrentN);
	
}


float angle( int from_x, int from_y, int to_x, int to_y)
{
	return atan2(from_x - to_x, to_y - from_y ) * 57.2957795;
}

int check_parents(Element *CurrentN)
{
	Element *tempN;
	Element *nextN;
	int dist;
	int tel=0;

	tempN = CurrentN;

	while( tempN->parent != NULL  )
	{
		if( CurrentN->depth%2 == 1)
		{
			nextN =  tempN->parent;
			dist = (CurrentN->own_robot.getX() - nextN->own_robot.getX()) *
			  	  (CurrentN->own_robot.getX() - nextN->own_robot.getX()) +
			  	  (CurrentN->own_robot.getY() - nextN->own_robot.getY()) *
			  	  (CurrentN->own_robot.getY() - nextN->own_robot.getY());

			if(dist < DIFF_DIST && CurrentN->own_robot.getRotation() == nextN->own_robot.getRotation() )
			{
				return 1;
			}
		}
		else
		{
			nextN =  tempN->parent;
			dist = (CurrentN->enemy_robot.getX() - nextN->enemy_robot.getX()) *
				  (CurrentN->enemy_robot.getX() - nextN->enemy_robot.getX()) +
				  (CurrentN->enemy_robot.getY() - nextN->enemy_robot.getY()) *
				  (CurrentN->enemy_robot.getY() - nextN->enemy_robot.getY());

			if(dist < DIFF_DIST && CurrentN->enemy_robot.getRotation() == nextN->enemy_robot.getRotation() )
			{
				return 1;
			}
		}
		tempN = nextN;
	}
	return 0;

}



int minimax(Element *CurrentN, int depth, int step)
{
	if( depth < MAX_DEPTH )
	{
		Element *tempN;
		
	
		//tempN = nieuwe locatie door met step_move vooruit te gaan
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[0]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjustlocation( tempN, step, depth%2);
		tempN->depth = depth+1;		

    		if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			GLO_del++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;		
			tempN->nr = GLO_nr;
			GLO_nr++;		 
			minimax(tempN, depth+1, step);
		}
		
		
		//tempN = nieuwe locatie door met step_move achteruit te gaan
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[1]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjustlocation( tempN, -step, depth%2);
		tempN->depth = depth+1;		
       	        if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			GLO_del++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;		
			tempN->nr = GLO_nr;
			GLO_nr++;		 
			minimax(tempN, depth+1, step);
		}
		
		
		//tempN = nieuwe locatie door te roteren met 15°
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[2]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjust_angle( tempN, 1, depth%2);
		tempN->depth = depth+1;		
        	if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			GLO_del++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;		
			tempN->nr = GLO_nr;
			GLO_nr++;		 
			minimax(tempN, depth+1, step);
		}
	
		
		
		//tempN = nieuwe locatie door te roteren met -15°
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[3]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjust_angle( tempN, -1, depth%2);
		tempN->depth = depth+1;		
        	if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			GLO_del++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;		
			tempN->nr = GLO_nr;
			GLO_nr++;		 
			minimax(tempN, depth+1, step);
		}
		
		
		if( CurrentN->next[0] == NULL &&  CurrentN->next[1] == NULL &&  CurrentN->next[2] == NULL && CurrentN->next[3] == NULL )
		{
			
			calculate_cost(CurrentN);
			return 0;
		}

		//alle takken bezocht -> kiezen welke waarde naar boven te brengen
		int i;
		float large=0;
		float small=99999;

		for(i=0;i<4;i++)
		{
			if( CurrentN->depth%2 != 0 && CurrentN->next[i] != NULL )
			{
				//printf("  ->  %d\n",CurrentN->next[i]->cost);
				if( CurrentN->next[i]->cost > large )
				{
					large = CurrentN->next[i]->cost;
					CurrentN->cost = large;
					CurrentN->child = CurrentN->next[i];
									
				}
	
				
			}
			else if( CurrentN->depth%2 == 0 && CurrentN->next[i] != NULL )
			{
				//printf("  ->  %d\n",CurrentN->next[i]->cost);
				if( CurrentN->next[i]->cost < small )
				{
					small = CurrentN->next[i]->cost;
					CurrentN->cost = small;
					CurrentN->child = CurrentN->next[i];
									
				}

			}
		}

		
	
	}
	else
	{
  		calculate_cost(CurrentN);
		
	}

}


void calculate_cost(Element *CurrentN)
{
	float angle_lw;
	float angle_lr;

	float angle_rw;
	float angle_rr;

	angle_lw = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), 0, 525);
	angle_lr = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), CurrentN->enemy_robot.getX(), CurrentN->enemy_robot.getY()-125);
	angle_rw = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), 0, 975);
	angle_rr = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), CurrentN->enemy_robot.getX(), CurrentN->enemy_robot.getY()+125);

	if( fabs(angle_lw - angle_lr) < fabs(angle_rw - angle_rr) )
	{
		CurrentN->cost =   fabs( CurrentN->own_robot.getRotation() - (fabs(angle_rw - angle_rr)/5 + angle_rw) );
	}
	else
	{
		CurrentN->cost =  fabs( CurrentN->own_robot.getRotation() - (  angle_lw - fabs(angle_lw - angle_lr)/5 ) );  
	}
	//CurrentN->cost += CurrentN->own_robot.getX()/100;
}



int move_size(Element *CurrentN)
{
	int x_dis;
	int move;
	if( GOAL == 0 )
	{
		x_dis = CurrentN->own_robot.getX();
	}
	else
	{
		x_dis = 1800 - CurrentN->own_robot.getX();
	}
	move = x_dis / MAX_DEPTH;

	//steeds met minimum 2.5cm verplaatsen
	if( move < 25)
		move = 25;
	//en maximum met de lengte van de robot
	if( move > 150 )
		move = 150;
	return move;
}

void adjust_angle(Element *node,int step, int flag)
{
	int hoek;

	if( flag == 0 )
	{
		hoek = 	(node->own_robot.getRotation() + step)%360;
		if( hoek < 0 )
			hoek = 360 + hoek;		
		node->own_robot.setRotation( hoek );
	}
	else
	{
		hoek = 	(node->enemy_robot.getRotation() + step)%360;
		if( hoek < 0 )
			hoek = 360 + hoek;
		node->enemy_robot.setRotation( hoek);
	}
}

void adjustlocation(Element *node, int step, int flag)
{
	double x,y;
	int x_int,y_int;

	if( flag == 0 )
	{
		x = -node->own_robot.getRotation();
		x = x / 57.2957795;
		x = sin(x);
		x = x * step;
		x = x + node->own_robot.getX();

		y = node->own_robot.getRotation();
		y = y / 57.2957795;
		y = cos(y);
		y = y * step;
		y = y + node->own_robot.getY();

		x_int = (int)x;
		y_int = (int)y;

		if( x_int <= 75 )
			x_int = 75;
		if( y_int <= 75 )
			y_int = 75;

		if( x_int >= 1725 )
			x_int = 1725;
		if( y_int >= 1425 )
			y_int = 1425;



		node->own_robot.setX( x_int );
		node->own_robot.setY( y_int );

	}
	else
	{
		x = -node->enemy_robot.getRotation();
		x = x / 57.2957795;
		x = sin(x);
		x = x * step;
		x = x + node->enemy_robot.getX();

		y = node->enemy_robot.getRotation();
		y = y / 57.2957795;
		y = cos(y);
		y = y * step;
		y = y + node->enemy_robot.getY();

		x_int = (int)x;
		y_int = (int)y;

		if( x_int <= 75 )
			x_int = 75;
		if( y_int <= 75 )
			y_int = 75;

		if( x_int >= 1725 )
			x_int = 1725;
		if( y_int >= 1425 )
			y_int = 1425;

		node->enemy_robot.setX( x_int );
		node->enemy_robot.setY( y_int );
	}

}








